#include <iostream>
#include <opencv2/core.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/highgui.hpp>

cv::Mat _filter_2d(const cv::Mat& src, const cv::Mat& kernel, cv::Point anchor, double delta, int borderType) {
    CV_Assert(src.type() == CV_8UC1);
    CV_Assert(kernel.type() == CV_32FC1);
    CV_Assert(kernel.rows % 2 == 1 && kernel.cols % 2 == 1);
    CV_Assert(anchor.x == -1 || (anchor.x >= 0 && anchor.x < kernel.cols));
    CV_Assert(anchor.y == -1 || (anchor.y >= 0 && anchor.y < kernel.rows));
    
    cv::Mat dst(src.size(), src.type());
    int top = anchor.y == -1 ? kernel.rows / 2 : anchor.y;
    int bottom = anchor.y == -1 ? kernel.rows / 2 : kernel.rows - 1 - anchor.y;
    int left = anchor.x == -1 ? kernel.cols / 2 : anchor.x;
    int right = anchor.x == -1 ? kernel.cols / 2 : kernel.cols - 1 - anchor.x;
    cv::Mat srcPadded;
    cv::copyMakeBorder(src, srcPadded, top, bottom, left, right, borderType);
    for (int i = 0; i < src.rows; i++) {
        uchar* p = dst.ptr<uchar>(i);
        for (int j = 0; j < src.cols; j++) {
            cv::Mat roi(srcPadded, cv::Rect(j, i, kernel.cols, kernel.rows));
            cv::Mat roiFloat;
            roi.convertTo(roiFloat, CV_32FC1);
            cv::Mat dstFloat;
            cv::multiply(roiFloat, kernel, dstFloat);
            cv::Scalar s = cv::sum(dstFloat);
            p[j] = cv::saturate_cast<uchar>(s[0] + delta);
        }
    }
    return dst;
}

cv::Mat _sep_filter_2d(const cv::Mat& src, const cv::Mat& kernel_x, const cv::Mat& kernel_y, cv::Point anchor, double delta, int borderType) {
    // ignore CV_Assert
    cv::Mat dst(src.size(), src.type());
    int top = anchor.y == -1 ? kernel_y.rows / 2 : anchor.y;
    int bottom = anchor.y == -1 ? kernel_y.rows / 2 : kernel_y.rows - 1 - anchor.y;
    int left = anchor.x == -1 ? kernel_x.cols / 2 : anchor.x;
    int right = anchor.x == -1 ? kernel_x.cols / 2 : kernel_x.cols - 1 - anchor.x;
    cv::Mat srcPadded;
    cv::copyMakeBorder(src, srcPadded, 0, 0, left, right, borderType);
    cv::Mat dst_temp(src.size(), CV_32FC1);
    for (int i = 0; i < src.rows; i++) {
        float* p = dst_temp.ptr<float>(i);
        for (int j = 0; j < src.cols; j++) {
            cv::Mat roi(srcPadded, cv::Rect(j, i, kernel_x.cols, 1));
            cv::Mat roiFloat;
            roi.convertTo(roiFloat, CV_32FC1);
            cv::Mat dstFloat;
            cv::multiply(roiFloat, kernel_x, dstFloat);
            cv::Scalar s = cv::sum(dstFloat);
            p[j] = cv::saturate_cast<float>(s[0]);
        }
    }
    cv::copyMakeBorder(dst_temp, srcPadded, top, bottom, 0, 0, borderType);
    //std::cout << (srcPadded.type() == CV_32FC1) << std::endl;
    for (int i = 0; i < src.rows; i++) {
        uchar* p = dst.ptr<uchar>(i);
        for (int j = 0; j < src.cols; j++) {
            cv::Mat roi(srcPadded, cv::Rect(j, i, 1, kernel_y.rows));
            cv::Mat dstFloat;
            cv::multiply(roi, kernel_y, dstFloat);
            cv::Scalar s = cv::sum(dstFloat);
            p[j] = cv::saturate_cast<uchar>(s[0] + delta);
        }
    }
    return dst;
}

int main(int argc, char **argv) {
    cv::Mat src = cv::imread("/home/xlll/Downloads/opencv/samples/data/lena.jpg", cv::IMREAD_GRAYSCALE);
    
    cv::Mat dst_filter2D;
    int ddepth = src.type();
    cv::Mat kernel_x = (cv::Mat_<float>(1, 3) << 1, 2, 0);
    cv::Mat kernel_y = (cv::Mat_<float>(3, 1) << -1, 0, 2);
    cv::Mat kernel = kernel_y * kernel_x;
    cv::Point anchor(-1, -1);
    double delta = 0;
    int borderType = cv::BORDER_REFLECT101;
    cv::filter2D(src, dst_filter2D, ddepth, kernel, anchor, delta, borderType);
    cv::Mat my_dst_filter2D = _filter_2d(src, kernel, anchor, delta, borderType);
    cv::Mat diff_filter2D = my_dst_filter2D != dst_filter2D;
    std::vector<cv::Point> nonzeros_filter2D;
    cv::findNonZero(diff_filter2D, nonzeros_filter2D);
    cv::imshow("src", src);
    cv::imshow("dst_filter2D", dst_filter2D);
    std::cout << "nonzeros_filter2D = " << nonzeros_filter2D << std::endl;
    
    cv::Mat dst_sepFilter2D;
    cv::sepFilter2D(src, dst_sepFilter2D, ddepth, kernel_x, kernel_y, anchor, delta, borderType);
    cv::Mat diff_filter2D_sepFilter2D = dst_sepFilter2D != dst_filter2D;
    std::vector<cv::Point> nonzeros_filter2D_sepFilter2D;
    cv::findNonZero(diff_filter2D_sepFilter2D, nonzeros_filter2D_sepFilter2D);
    std::cout << "nonzeros_filter2D_sepFilter2D = " << nonzeros_filter2D_sepFilter2D << std::endl;
    
    cv::Mat my_dst_sepFilter2D = _sep_filter_2d(src, kernel_x, kernel_y, anchor, delta, borderType);
    cv::Mat diff_sepFilter2D = my_dst_sepFilter2D != dst_sepFilter2D;
    std::vector<cv::Point> nonzeros_sepFilter2D;
    cv::findNonZero(diff_sepFilter2D, nonzeros_sepFilter2D);
    std::cout << "nonzeros_sepFilter2D = " << nonzeros_sepFilter2D << std::endl;
    
    cv::waitKey(0);
    return 0;
}
